#include "pvmodel.h"

//DCside
Switch Bre_PV, Bre_Chop, Bre_Char, Boost_IGBT, Boost_Diode;
Capacitor C_PV, C_Boost, C_VSC;
Inductor L_Boost;

//ACside
Switch Bre_PVunit;
VSC VSC_average;
Capacitor C_filt[3];
Inductor L_filt[3];

//PV arrays
static double dV, V_PV_V_his;
static double parPV[2];
static double dk;
static double V_PV_V, I_PV_A;
static double V_PV_kV, I_PV_kA;
static double Npar, Nser;
static double Isc1_PV, par_PV_b1, par_PV_c1, Voc1_PV;
static int Count_k;

//Boost
static int STATUS_his[2];

//Y Calculation (DC side)
static Matrix <double, NODE_NUM_DC, BRANCH_NUM_DC> A2;
static Matrix <double, BRANCH_NUM_DC, BRANCH_NUM_DC> Yg2;
static Matrix <double, NODE_NUM_DC, 1> V_DC;

//LC Filter
static double G_filt_R1, G_filt_R2;

//Transformer
static Matrix<double, 2, 2> G_trans;
static Matrix<double, 2, 2> parB_trans;
static Matrix<double, 2, 1> J_trans_A, J_trans_B, J_trans_C;

//Y Calculation(AC side)
static Matrix <double, NODE_NUM_AC, BRANCH_NUM_AC> A1;
static Matrix <double, BRANCH_NUM_AC, BRANCH_NUM_AC> Yg1;

static Matrix <double, NODE_NUM_AC - 3, 3> Y21;
static Matrix <double, NODE_NUM_AC - 3, NODE_NUM_AC - 3> Y22_inv;
static Matrix <double, NODE_NUM_AC - 3, 1> Jin;

int pvmodel_pre(double* _G_PVunit, double* _INJ_ABC, double* _V_ABC, double* _DELT, 
    double* _PVarray_pars, double* _Boost_pars, double* _CPB_pars, double* _VSC_pars, double* _LC_pars, double* _PVunit_Bre_pars, double* _Trans_pars, double* _DATA_OUT)
{
	
	//Initializing parameter
	
    //////////////////////////DC Side/////////////////////////////
    //PV
	//PV Breaker [intial status: OPEN]
	double Roff_PV = _PVarray_pars[14];
	double Ron_PV = _PVarray_pars[15];
	//Compared with RTDS?
	Bre_PV.gon = 1.0 / Ron_PV;
	Bre_PV.goff = 1.0 / Roff_PV;
	Bre_PV.g = Bre_PV.goff;

	//PV Capacitor
	double CPV = _PVarray_pars[16];

	C_PV.cap_g = 2.0 * CPV * 1.0e-6 / *_DELT;
	C_PV.cap_jh = 0.0;
	C_PV.cap_ib = 0.0;
	C_PV.cap_v = 0.0;

	//PVarray
	double Voc_PV, Isc_PV, Vm_PV, Im_PV;
	double Tref, Sref;
	double INSO_ini, TEMP_ini, Vpv_ini;
	double par_PV_a, par_PV_b, par_PV_c;

	Voc_PV = _PVarray_pars[0];
	Isc_PV = _PVarray_pars[1];
	Vm_PV = _PVarray_pars[2];
	Im_PV = _PVarray_pars[3];
	Npar = _PVarray_pars[4];
	Nser = _PVarray_pars[5];
	Tref = _PVarray_pars[6];
	Sref = _PVarray_pars[7];
	INSO_ini = _PVarray_pars[8];
	TEMP_ini = _PVarray_pars[9]; 
	Vpv_ini = _PVarray_pars[10];
	par_PV_a = _PVarray_pars[11];
	par_PV_b = _PVarray_pars[12];
	par_PV_c = _PVarray_pars[13];

	double Im1_PV, Vm1_PV;
	double dTemp, SdSref, dS;

	dTemp = TEMP_ini - Tref;
	SdSref = INSO_ini / Sref;
	dS = SdSref - 1.0;

	//Modified Expression I'sc V'oc I'm V'm
	Isc1_PV = Isc_PV * SdSref * (par_PV_a * dTemp + 1.0);
	Im1_PV = Im_PV * SdSref * (par_PV_a * dTemp + 1.0);
	Voc1_PV = Voc_PV * (1.0 - dTemp * par_PV_c) * log(M_E + par_PV_b * dS);
	Vm1_PV = Vm_PV * (1.0 - dTemp * par_PV_c) * log(M_E + par_PV_b * dS);

	//Calculate C1(par_PV_b1) and C2(par_PV_c1)
	par_PV_c1 = (Vm1_PV / Voc1_PV - 1.0) / log(1.0 - Im1_PV / Isc1_PV);
	par_PV_b1 = (1 - Im1_PV / Isc1_PV) * exp(Vm1_PV / (-1.0 * par_PV_c1 * Voc1_PV));

	parPV[0] = -(par_PV_b1 + 1.0) * Isc1_PV;
	parPV[1] = 1.0 / (par_PV_c1 * Voc1_PV);

	V_PV_V_his = Vpv_ini * 1000.0 / Nser;
	// I = Isc * (1 - C1 * (exp(V / C2 / Voc)-1))
	I_PV_A = Isc1_PV * (1.0 - par_PV_b1 * (exp(V_PV_V_his / par_PV_c1 / Voc1_PV) - 1.0));

	Count_k = 0;

	//Boost
	//Boost Inductor [unit: H]
	double LBoost = _Boost_pars[0];

	L_Boost.ind_g = *_DELT / (2.0 * LBoost);
	L_Boost.ind_jh = 0.0;
	L_Boost.ind_ib = 0.0;
	L_Boost.ind_v = 0.0;

	//Boost Capacitor [unit: uF]
	double CBoost = _Boost_pars[1];

	C_Boost.cap_g = 2.0 * CBoost * 1.0e-6 / *_DELT;
	C_Boost.cap_jh = 0.0;
	C_Boost.cap_ib = 0.0;
	C_Boost.cap_v = 0.0;

	//IGBT [intial status: OFF]
	double Ron_IGBT = _Boost_pars[2];
	double Roff_IGBT = _Boost_pars[3];

	Boost_IGBT.gon = 1.0 / Ron_IGBT;
	Boost_IGBT.goff = 1.0 / Roff_IGBT;
	Boost_IGBT.g = Boost_IGBT.goff;

	//Diode [intial status: OFF]
	double Ron_Diode = _Boost_pars[4];
	double Roff_Diode = _Boost_pars[5];

	Boost_Diode.gon = 1.0 / Ron_Diode;
	Boost_Diode.goff = 1.0 / Roff_Diode;
	Boost_Diode.g = Boost_IGBT.goff;

	//Boost STATUS  0:Diode  1:IGBT
	STATUS_his[0] = 0;
	STATUS_his[1] = 0;

	//Chopper Branch [intial status: OPEN]
	double Roff_Chop = _CPB_pars[0];
	double Ron_Chop = _CPB_pars[1];

	Bre_Chop.gon = 1.0 / Ron_Chop;
	Bre_Chop.goff = 1.0 / Roff_Chop;
	Bre_Chop.g =  Bre_Chop.goff;

	//Precharge Branch[intial status: OPEN]
	double Roff_Char = _CPB_pars[2];
	double Ron_Char = _CPB_pars[3];

	Bre_Char.gon = 1.0 / Ron_Char;
	Bre_Char.goff = 1.0 / Roff_Char;
	Bre_Char.g =  Bre_Char.gon;
	
	//ATTETION
	double j_Char = 0.0;

	//VSC
	//DC Bus Capacitor
	double CBus = _VSC_pars[3];

	C_VSC.cap_g = 2.0 * CBus * 1.0e-6 / *_DELT;
	C_VSC.cap_jh = 0.0;
	C_VSC.cap_ib = 0.0;
	C_VSC.cap_v = 0.0;

    //////////////////////////AC Side/////////////////////////////
    double Ron_VSC = _VSC_pars[0];
	double Roff_VSC = _VSC_pars[1];
	double Rsnubber = _VSC_pars[2];
	
	VSC_average.gon_vsc = 1.0 / Ron_VSC;
	VSC_average.goff_vsc = 1.0 / Roff_VSC;
	VSC_average.g_snubber = 1.0 / Rsnubber;
	VSC_average.g_vsc = VSC_average.g_snubber;

	//LC filter
	double L_LC = _LC_pars[0];
	double R1_LC = _LC_pars[1];
	double C_LC = _LC_pars[2];
	double R2_LC = _LC_pars[3];

	G_filt_R1 = 1.0 / R1_LC;
	G_filt_R2 = 1.0 / R2_LC;

	//Inductor
	for (int i = 0; i < 3; i++)
	{
		L_filt[i].ind_g = *_DELT / (2.0 * L_LC);
		L_filt[i].ind_v = 0.0;
		L_filt[i].ind_ib = 0.0;
		L_filt[i].ind_jh = 0.0;
	}
	
	//Capacitor
	for (int i = 0; i < 3; i++)
	{
		C_filt[i].cap_g = 2.0 * C_LC * 1.0e-6 / *_DELT;
		C_filt[i].cap_v = 0.0;
		C_filt[i].cap_ib = 0.0;
		C_filt[i].cap_jh = 0.0;
	}

	//PV unit breaker
	double Roff_PVunit = _PVunit_Bre_pars[0];
	double Ron_PVunit = _PVunit_Bre_pars[1];
	
	Bre_PVunit.gon = 1.0 / Ron_PVunit;
	Bre_PVunit.goff = 1.0 / Roff_PVunit;
	Bre_PVunit.g = Bre_PVunit.goff;

	//Transformer
	double Sb_trans = _Trans_pars[0];
	double fb_trans = _Trans_pars[1];
	double XL_trans = _Trans_pars[2];
	double Z_trans = _Trans_pars[3];
	double Im_trans = _Trans_pars[4];
	double V1_trans = _Trans_pars[5];
	double V2_trans = _Trans_pars[6];

	double N_trans = V1_trans / V2_trans;
	double Zb1_trans = V1_trans * V1_trans / Sb_trans;
	double Zb2_trans = V2_trans * V2_trans / Sb_trans;

	double L1_trans = (XL_trans / 2) * Zb1_trans / (2 * PI * fb_trans);
	double L2_trans = L1_trans / N_trans / N_trans;
	double Lm_trans = 1.0 / Im_trans * Zb1_trans / (2 * PI * fb_trans); 

	double R1_trans = Z_trans / 2 * Zb1_trans;
	double R2_trans = Z_trans / 2 * Zb2_trans;

	Matrix<double, 2, 2> R_trans;
	Matrix<double, 2, 2> L_trans;

	R_trans.setZero();
	R_trans(0, 0) = R1_trans;
	R_trans(1, 1) = R2_trans;

	L_trans(0, 0) = L1_trans + Lm_trans;
	L_trans(0, 1) = Lm_trans / N_trans;
	L_trans(1, 0) = Lm_trans / N_trans;
	L_trans(1, 1) = L2_trans + Lm_trans / N_trans / N_trans;

	Matrix<double, 2, 2> parA_trans;

	parA_trans = R_trans + 2 / (*_DELT) * L_trans;
	G_trans = parA_trans.inverse();
	parB_trans = -R_trans +  2 / (*_DELT) * L_trans;

	J_trans_A.setZero();
	J_trans_B.setZero();
	J_trans_C.setZero();

	//Y Calculation
    //Read A1
	//Open file
	FILE* fp1 = fopen("../A1.txt", "r");
	if(fp1 == NULL)
	{
		std::cerr << "Cannot open A1.txt\n";
        return 1;
	}

	int rows1, cols1;
	fscanf(fp1, "%d %d", &rows1, &cols1);

	for (int i = 0; i < rows1; i++) {
        for (int j = 0; j < cols1; j++) {
            double value;
            fscanf(fp1, "%lf", &value);
            A1(i, j) = value;
        }
    }

	fclose(fp1);

	cout << "A1:" <<endl;
	cout << A1 << endl;

	//Read A2
	//Open file
	FILE* fp2 = fopen("../A2.txt", "r");
	
	if(fp2 == NULL)
	{
		std::cerr << "Cannot open A2.txt\n";
        return 1;
	}

	int rows2, cols2;

	fscanf(fp2, "%d %d", &rows2, &cols2);

	for (int i = 0; i < rows2; i++) {
        for (int j = 0; j < cols2; j++) {
            double value;
            fscanf(fp2, "%lf", &value);
            A2(i, j) = value;
        }
    }

	fclose(fp2);

	cout << "A2:" <<endl;
	cout << A2 << endl;

	//Generate Yg
	//AC side
	Yg1 = Matrix<double, BRANCH_NUM_AC, BRANCH_NUM_AC>::Zero();
	//Transformer
	for(int i = 0; i < 3; i++)
	{
		Yg1(i * 6, i * 6) = G_trans(1, 1);
		Yg1(i * 6 + 1, i * 6 + 1) = -G_trans(0, 1);
		Yg1(i * 6 + 2, i * 6 + 2) = G_trans(0, 1);
		Yg1(i * 6 + 3, i * 6 + 3) = G_trans(0, 1);
		Yg1(i * 6 + 4, i * 6 + 4) = -G_trans(0, 1);
		Yg1(i * 6 + 5, i * 6 + 5) = G_trans(0, 0);
	}
	
	//PVunit Breaker
	Yg1(18, 18) = Bre_PVunit.g;
	Yg1(19, 19) = Bre_PVunit.g;
	Yg1(20, 20) = Bre_PVunit.g;
	
	//LC filter [NO CHANGE]
	for(int i = 0; i < 3; i++)
	{
		Yg1(i + 21, i + 21) = G_filt_R2;
		Yg1(i + 24, i + 24) = C_filt[i].cap_g;
		Yg1(i + 27, i + 27) = G_filt_R1;
		Yg1(i + 30, i + 30) = L_filt[i].ind_g;
	}

	//VSC [initial status: off] [CHANGE]
	for(int i = 33; i < 36; i++)
	{
		Yg1(i, i) = VSC_average.g_vsc;
	}

	//DC side
	Yg2 = Matrix<double, BRANCH_NUM_DC, BRANCH_NUM_DC>::Zero();
	Yg2(0, 0) = C_VSC.cap_g; //[NO CHANGE]

	//Precharge and Chopper
	Yg2(1, 1) = Bre_Char.g;
	Yg2(2, 2) = Bre_Chop.g;

	//Boost
	Yg2(3, 3) = C_Boost.cap_g;
	Yg2(4, 4) = Boost_Diode.g;
	Yg2(5, 5) = Boost_IGBT.g;
	Yg2(6, 6) = L_Boost.ind_g;

	//PV array
	Yg2(7, 7) = Bre_PV.g;
	Yg2(8, 8) = C_PV.cap_g;

	//Generate J_branch
	//AC side
	Matrix <double, BRANCH_NUM_AC, 1> J_branch1;
	J_branch1 = Matrix<double, BRANCH_NUM_AC, 1>::Zero();

	J_branch1(0) = J_trans_A(1);
	J_branch1(5) = J_trans_A(0);
	J_branch1(6) = J_trans_B(1);
	J_branch1(11) = J_trans_B(0);
	J_branch1(12) = J_trans_C(1);
	J_branch1(17) = J_trans_C(0);

	J_branch1(24) = C_filt[0].cap_jh;
	J_branch1(25) = C_filt[1].cap_jh;
	J_branch1(26) = C_filt[2].cap_jh;

	J_branch1(30) = L_filt[0].ind_jh;
	J_branch1(31) = L_filt[1].ind_jh;
	J_branch1(32) = L_filt[2].ind_jh;

	//DC side
	Matrix <double, BRANCH_NUM_DC, 1> J_branch2;
	J_branch2 = Matrix<double, BRANCH_NUM_DC, 1>::Zero();

	J_branch2(0) = C_VSC.cap_jh;

	J_branch2(1) = j_Char; //Depend on Signal

	J_branch2(3) = C_Boost.cap_jh;
	J_branch2(6) = L_Boost.ind_jh;

	//ATTENTION //Please Check
	I_PV_kA = I_PV_A * Npar * 0.001;
	J_branch2(8) = C_PV.cap_jh + I_PV_kA;

	//交直流侧分块
	Matrix<double, NODE_NUM_AC, NODE_NUM_AC> Y1;
	Matrix<double, NODE_NUM_AC, 1> J1;
	Matrix<double, NODE_NUM_DC, NODE_NUM_DC> Y2;
	Matrix<double, NODE_NUM_DC, 1> J2;

	Y1 = A1 * Yg1 * A1.transpose();
    Y2 = A2 * Yg2 * A2.transpose();
    J1 = - A1 * J_branch1;
    J2 = - A2 * J_branch2;

	//Y_sum and J_sum
	Matrix<double, 3, 3> Y11;
	Matrix<double, 3, NODE_NUM_AC - 3> Y12;
	Matrix<double, NODE_NUM_AC - 3, NODE_NUM_AC - 3> Y22;
	Matrix<double, 3, 1> Jex;

	Y12.setZero();
	Y11 = Y1.topLeftCorner(3, 3);
	Y12.topRightCorner(3, NODE_NUM_AC - 3) = Y1.topRightCorner(3, NODE_NUM_AC - 3);
	Y22 = Y1.bottomRightCorner(NODE_NUM_AC - 3, NODE_NUM_AC -3);

	Y21 = Y12.transpose();
	Y22_inv = Y22.inverse();

	Jex = J1.topRows(3);
	Jin = J1.bottomRows(NODE_NUM_AC - 3);
    
	//Calculate Yeq and Jeq
	Matrix<double, 3, 3> Yeq;
	Matrix<double, 3, 1> Jeq;

	Yeq = Y11 - Y12 * Y22_inv * Y21;
	Jeq = Jex - Y12 * Y22_inv * Jin;

	cout << "Yeq:" << endl;
	cout << Yeq << endl;
	cout << "Jeq:" << endl;
	cout << Jeq << endl;

	//Calculate DC side voltage (Y2)
	V_DC = Y2.inverse() * J2;

	cout << "V_DC:" << endl;
	for (int i = 0; i < 4; i++)
	{
		cout << V_DC(i) << endl;
	}

	//Transfer to branch mode
	_INJ_ABC[0] = -Jeq(0);
	_INJ_ABC[1] = -Jeq(1);
	_INJ_ABC[2] = -Jeq(2);

	//Check!!!!!
	_G_PVunit[0] = Yeq(0, 0) + Yeq(1, 0) + Yeq(2, 0);
	_G_PVunit[1] = Yeq(1, 1) + Yeq(0, 1) + Yeq(2, 1);
	_G_PVunit[2] = Yeq(2, 2) + Yeq(0, 2) + Yeq(1, 2);
	_G_PVunit[3] = -Yeq(1, 0);
	_G_PVunit[4] = -Yeq(2, 1);
	_G_PVunit[5] = -Yeq(2, 0);

	cout << "_G_PVunit:" << endl;

	for (int i = 0; i < 6; i++)
	{
		cout << _G_PVunit[i] << endl;
	}

	/*OUTPUT DATA*/
    //DC Side
	_DATA_OUT[0] = V_PV_kV; //VpvBus
	_DATA_OUT[1] = I_PV_kA; //Ipv
	_DATA_OUT[2] = C_VSC.cap_v; //VdcBus
    //AC Side
    _DATA_OUT[3] = 0;
	_DATA_OUT[4] = 0;
	_DATA_OUT[5] = 0;
	_DATA_OUT[6] = 0;
	_DATA_OUT[7] = 0;
	_DATA_OUT[8] = 0; 
	_DATA_OUT[9] = 0;

	return 0;

}

int pvmodel(double* _G_PVunit, double* _INJ_ABC, double* _V_ABC, 
    int* _Dblk1, int* _SW_AC, int* _SW_DC, double* _Vchar, double* _DATA_OUT)
{
	
    //////////////////////////DC Side/////////////////////////////
    //SIGNAL
	int SW_BRE_PV, SW_BRE_Chop, SW_BRE_Char;
	int SW_Boost;

	SW_BRE_PV = _SW_DC[0];
	SW_BRE_Chop = _SW_DC[1];
	SW_BRE_Char = _SW_DC[2];
	SW_Boost = _SW_DC[3];

	//VSC Capacitor
	C_VSC.cap_v = V_DC(0) - 0;

	//Boost
	C_Boost.cap_v = V_DC(0) - 0;
	L_Boost.ind_v = V_DC(2) - V_DC(1);

	//PV array Capacitor
	C_PV.cap_v = 0 - V_DC(3);

	//Calculate Branch Current
	//Capacitor
	C_VSC.cap_ib = C_VSC.cap_g * C_VSC.cap_v + C_VSC.cap_jh;
	C_Boost.cap_ib = C_Boost.cap_g * C_Boost.cap_v + C_Boost.cap_jh;
	C_PV.cap_ib = C_PV.cap_g * C_PV.cap_v + C_PV.cap_jh;

	//Inductor
	L_Boost.ind_ib = L_Boost.ind_g * L_Boost.ind_v + L_Boost.ind_jh;

	//Update branch G
	//BREAKER
	Bre_PV.g = SW_BRE_PV * Bre_PV.goff + (1 - SW_BRE_PV) * Bre_PV.gon;
	Bre_Chop.g = SW_BRE_Chop * Bre_Chop.goff + (1 - SW_BRE_Chop) * Bre_Chop.gon;
	Bre_Char.g = SW_BRE_Char * Bre_Char.goff + (1 - SW_BRE_Char) * Bre_Char.gon;

	//Boost
	double iLcheck = 2 * L_Boost.ind_g * L_Boost.ind_v + L_Boost.ind_ib;
	int STATUS[2];

	//IGBT
	if(SW_Boost == 1)
	{
		STATUS[1] = 1;
	}
	else
	{
		STATUS[1] = 0;
	}

	//Diode
	STATUS[0] = 0;
	if(STATUS[1] == 0)
	{
		if((STATUS_his[1] == 1) ||(STATUS_his[0] == 1 && iLcheck > 0))
		{
			STATUS[0] = 1;
		}
	}

	Boost_IGBT.g = STATUS[1] * Boost_IGBT.gon + (1 - STATUS[1]) * Boost_IGBT.goff;
	Boost_Diode.g = STATUS[0] * Boost_Diode.gon + (1 - STATUS[0]) * Boost_Diode.goff;

	STATUS_his[0] = STATUS[0];
	STATUS_his[1] = STATUS[1];

    //////////////////////////AC Side/////////////////////////////
    //SIGNAL
	int SW_VSC[6];

	for(int i = 0; i < 6; i++)
	{
		SW_VSC[i] = _SW_AC[i];
	}

	int SW_PVunit;
	SW_PVunit = _SW_AC[6];
	
	//DSOUT
	Matrix <double, 3, 1> Vex;
	Matrix <double, NODE_NUM_AC - 3, 1> Vin;

	//Vex
	for(int i = 0; i < 3; i++)
	{
		Vex(i) = _V_ABC[i];
	}

	//cout << "Y22_inv:" << endl;
	//cout << Y22_inv << endl;

	//Calculate Vin
	Vin = Y22_inv * (Jin - Y21 * Vex);

	//Calculate branch voltage and current
	//Transformer
	Matrix <double, 2, 1> V_port_A, V_port_B, V_port_C;
	Matrix <double, 2, 1> I_port_A, I_port_B, I_port_C;

	V_port_A(0) = Vin(0) - 0;
	V_port_B(0) = Vin(1) - 0;
	V_port_C(0) = Vin(2) - 0;

	V_port_A(1) = Vex(0) - Vex(1);
	V_port_B(1) = Vex(1) - Vex(2);
	V_port_C(1) = Vex(2) - Vex(0);

	//LC filter
	C_filt[0].cap_v = Vin(6) - 0;
	C_filt[1].cap_v = Vin(7) - 0;
	C_filt[2].cap_v = Vin(8) - 0;

	L_filt[0].ind_v = Vin(9) - Vin(12);
	L_filt[1].ind_v = Vin(10) - Vin(13);
	L_filt[2].ind_v = Vin(11) - Vin(14);

	//Calculate Branch Current
	//Transformer
	I_port_A = G_trans * V_port_A + J_trans_A;
	I_port_B = G_trans * V_port_B + J_trans_B;
	I_port_C = G_trans * V_port_C + J_trans_C;

	//Capacitor
	for(int i = 0; i < 3; i++)
	{
		C_filt[i].cap_ib = C_filt[i].cap_g * C_filt[i].cap_v + C_filt[i].cap_jh;
	}

	//Inductor
	for(int i = 0; i < 3; i++)
	{
		L_filt[i].ind_ib = L_filt[i].ind_g * L_filt[i].ind_v + L_filt[i].ind_jh;
	}

	//Update History Current
	//Transformer
	J_trans_A = G_trans * (V_port_A + parB_trans * I_port_A);
	J_trans_B = G_trans * (V_port_B + parB_trans * I_port_B);
	J_trans_C = G_trans * (V_port_C + parB_trans * I_port_C);

	//Capacitor
	for(int i = 0; i < 3; i++)
	{
		C_filt[i].cap_jh = - C_filt[i].cap_g * C_filt[i].cap_v - C_filt[i].cap_ib;
	}

	//VSC
	double idc_source;
	double jabc[3];

	if(*_Dblk1 == 0)
	{
		//DC side
		idc_source = 0;

		//AC side
		jabc[0] = (Vin(9) - Vin(10)) * VSC_average.g_vsc;
		jabc[1] = (Vin(10) - Vin(11)) * VSC_average.g_vsc;
		jabc[2] = (Vin(11) - Vin(9)) * VSC_average.g_vsc;

		//History Current
		for(int i = 0; i < 3; i++)
		{
			L_filt[i].ind_jh = 0;
		}
	}
	else
	{
		
		idc_source = - SW_VSC[0] * L_filt[0].ind_ib - SW_VSC[1] * L_filt[1].ind_ib - SW_VSC[2] * L_filt[2].ind_ib;

		jabc[0] = (SW_VSC[0] - SW_VSC[1]) * C_VSC.cap_v * VSC_average.g_vsc;
		jabc[1] = (SW_VSC[1] - SW_VSC[2]) * C_VSC.cap_v * VSC_average.g_vsc;
		jabc[2] = (SW_VSC[2] - SW_VSC[0]) * C_VSC.cap_v * VSC_average.g_vsc;

		//Inductor
		for(int i = 0; i < 3; i++)
		{
			L_filt[i].ind_jh = L_filt[i].ind_g * L_filt[i].ind_v + L_filt[i].ind_ib;
		}
	}

	//////////////////////////DC Side/////////////////////////////
	//Update History Current
	//Capacitor
	C_VSC.cap_jh = - C_VSC.cap_g * C_VSC.cap_v - C_VSC.cap_ib;
	C_Boost.cap_jh = - C_Boost.cap_g * C_Boost.cap_v - C_Boost.cap_ib;
	C_PV.cap_jh = - C_PV.cap_g * C_PV.cap_v - C_PV.cap_ib;

	//Inductor
	L_Boost.ind_jh = L_Boost.ind_g * L_Boost.ind_v + L_Boost.ind_ib;

	//PV array
	V_PV_kV = V_DC(3) - 0;
	V_PV_V = V_PV_kV * 1000.0 / Nser;
	V_PV_V_his = V_PV_V;
	I_PV_A = Isc1_PV * (1.0 - par_PV_b1 * (exp(V_PV_V_his / par_PV_c1 / Voc1_PV) - 1.0));
	I_PV_kA =  I_PV_A * Npar * 0.001;

	//Precharge Branch
	double j_Char = - (*_Vchar) * Bre_Char.g;

	
	//Gernate Yg
	//AC side
	Bre_PVunit.g = SW_PVunit * Bre_PVunit.goff + (1 - SW_PVunit) * Bre_PVunit.gon;

	Yg1(18, 18) = Bre_PVunit.g;
	Yg1(19, 19) = Bre_PVunit.g;
	Yg1(20, 20) = Bre_PVunit.g;

	//DC side
	//Precharge and Chopper //CHeck Bre_Char
	Yg2(1, 1) = Bre_Char.g;
	Yg2(2, 2) = Bre_Chop.g;

	//Boost
	Yg2(4, 4) = Boost_Diode.g;
	Yg2(5, 5) = Boost_IGBT.g;
	
	//PV array
	Yg2(7, 7) = Bre_PV.g;

	//Generate J
	//AC side
	Matrix <double, BRANCH_NUM_AC, 1> J_branch1;
	J_branch1 = Matrix<double, BRANCH_NUM_AC, 1>::Zero();

	J_branch1(0) = J_trans_A(1);
	J_branch1(5) = J_trans_A(0);
	J_branch1(6) = J_trans_B(1);
	J_branch1(11) = J_trans_B(0);
	J_branch1(12) = J_trans_C(1);
	J_branch1(17) = J_trans_C(0);

	J_branch1(24) = C_filt[0].cap_jh;
	J_branch1(25) = C_filt[1].cap_jh;
	J_branch1(26) = C_filt[2].cap_jh;

	J_branch1(30) = L_filt[0].ind_jh;
	J_branch1(31) = L_filt[1].ind_jh;
	J_branch1(32) = L_filt[2].ind_jh;

	J_branch1(33) = -jabc[0];
	J_branch1(34) = -jabc[1];
	J_branch1(35) = -jabc[2];
	
	//DC side
	Matrix <double, BRANCH_NUM_DC, 1> J_branch2;
	J_branch2 = Matrix<double, BRANCH_NUM_DC, 1>::Zero();

	J_branch2(0) = C_VSC.cap_jh + idc_source;

	J_branch2(1) = j_Char; //Depend on Signal

	J_branch2(3) = C_Boost.cap_jh;
	J_branch2(6) = L_Boost.ind_jh;

	//ATTENTION //Please Check
	J_branch2(8) = C_PV.cap_jh + I_PV_kA;

	//Calculate Y and J
	Matrix<double, NODE_NUM_AC, NODE_NUM_AC> Y1;
	Matrix<double, NODE_NUM_AC, 1> J1;
	Matrix<double, NODE_NUM_DC, NODE_NUM_DC> Y2;
	Matrix<double, NODE_NUM_DC, 1> J2;

	Y1 = A1 * Yg1 * A1.transpose();
    Y2 = A2 * Yg2 * A2.transpose();
    J1 = - A1 * J_branch1;
    J2 = - A2 * J_branch2;

	//Y_sum and J_sum
	Matrix<double, 3, 3> Y11;
	Matrix<double, 3, NODE_NUM_AC - 3> Y12;
	Matrix<double, NODE_NUM_AC - 3, NODE_NUM_AC - 3> Y22;
	Matrix<double, 3, 1> Jex;

	Y12.setZero();
	Y11 = Y1.topLeftCorner(3, 3);
	Y12.topRightCorner(3, NODE_NUM_AC - 3) = Y1.topRightCorner(3, NODE_NUM_AC - 3);
	Y22 = Y1.bottomRightCorner(NODE_NUM_AC - 3, NODE_NUM_AC -3);

	Y21 = Y12.transpose();
	Y22_inv = Y22.inverse();

	Jex = J1.topRows(3);
	Jin = J1.bottomRows(NODE_NUM_AC - 3);
    
	//Calculate Yeq and Jeq
	Matrix<double, 3, 3> Yeq;
	Matrix<double, 3, 1> Jeq;

	Yeq = Y11 - Y12 * Y22_inv * Y21;
	Jeq = Jex - Y12 * Y22_inv * Jin;

	//Calculate DC side voltage (Y2)
	V_DC = Y2.inverse() * J2;

	//cout << "Yeq:" << endl;
	//cout << Yeq << endl;
	//cout << "Jeq:" << endl;
	//cout << Jeq << endl;

	//cout << "Jeq:" << endl;
	//cout << Jeq << endl;

	//Transfer to branch mode
	_INJ_ABC[0] = -Jeq(0);
	_INJ_ABC[1] = -Jeq(1);
	_INJ_ABC[2] = -Jeq(2);

	_G_PVunit[0] = Yeq(0, 0) + Yeq(1, 0) + Yeq(2, 0);
	_G_PVunit[1] = Yeq(1, 1) + Yeq(1, 0) + Yeq(2, 1);
	_G_PVunit[2] = Yeq(2, 2) + Yeq(2, 1) + Yeq(2, 0);
	_G_PVunit[3] = -Yeq(1, 0);
	_G_PVunit[4] = -Yeq(2, 1);
	_G_PVunit[5] = -Yeq(2, 0);

	/*OUTPUT DATA*/
	_DATA_OUT[0] = V_PV_kV; //VpvBus
	_DATA_OUT[1] = I_PV_kA; //Ipv
	_DATA_OUT[2] = V_DC(0) - 0; //VdcBus
    _DATA_OUT[3] = idc_source; //idc_source
	_DATA_OUT[4] = Vin(0); // Vcon1A
	_DATA_OUT[5] = Vin(1); // Vcon1B
	_DATA_OUT[6] = Vin(2); // Vcon1C
	_DATA_OUT[7] = I_port_A(0); //Icon1A
	_DATA_OUT[8] = I_port_B(0); //Icon1B
	_DATA_OUT[9] = I_port_C(0); //Icon1C

	return 0;

}